#include "util/Matrix.h"
#include <algorithm>
#include <cmath>
#include <stdexcept>

namespace util {

Matrix::Matrix(std::size_t rows, std::size_t columns, GenericType)
  : rows_(rows), columns_(columns)
  , impl_(new GenericImpl(rows, columns))
{
}

Matrix::Matrix(std::size_t rows, std::size_t columns, SparseType)
  : rows_(rows), columns_(columns)
  , impl_(new SparseImpl(rows, columns))
{
}

Matrix::Matrix(std::size_t size, DiagonalType)
  : rows_(size), columns_(size)
  , impl_(new DiagonalImpl(size))
{
}

Matrix::Matrix(std::size_t size, const std::vector<Real>& diagonalValues, DiagonalType)
  : rows_(diagonalValues.size()), columns_(diagonalValues.size())
  , impl_(new DiagonalImpl(size, diagonalValues))
{
}

Matrix::Matrix(std::size_t size, SimmetricType)
  : rows_(size), columns_(size)
  , impl_(new SimmetricImpl(size))
{
}

Matrix::Matrix(std::size_t size, const std::vector<Real>& upperValues, SimmetricType)
  : rows_(size), columns_(size)
  , impl_(new SimmetricImpl(size, upperValues))
{
}

Matrix::Matrix(const Matrix& other)
  : rows_(other.rows_), columns_(other.columns_)
  , impl_(other.impl_->clone())
{
}

Matrix& Matrix::operator=(const Matrix& other)
{
  rows_ = other.rows_;
  columns_ = other.columns_;
  impl_ = std::auto_ptr<MatrixImpl>(other.impl_->clone());
  return *this;
}

bool Matrix::isSparse() const
{
  return dynamic_cast<const SparseImpl*>(impl_.get()) != 0;
}

bool Matrix::isDiagonal() const
{
  return dynamic_cast<const DiagonalImpl*>(impl_.get()) != 0;
}

bool Matrix::isSimmetric() const
{
  return dynamic_cast<const SimmetricImpl*>(impl_.get()) != 0;
}

bool Matrix::isGeneric() const
{
  return dynamic_cast<const GenericImpl*>(impl_.get()) != 0;
}

Matrix Matrix::transposed() const
{
  Matrix transp(rows_, columns_);
  for (std::size_t r = 0; r < rows_; ++r)
    for (std::size_t c = 0; c < columns_; ++c)
      transp(c, r) = getValue(r, c);
  return transp;
}

Matrix& Matrix::transpose()
{
  // TODO: Vedere un approccio tipo decorator (che scambia row e column per ogni chiamata)
  if (rows_ != columns_)
    throw std::logic_error("Not implemented for a rectangular matrix");
  // vedere quando la matrice è rettangolare
  // bisogna scambiare rows_ e columns_

  // valido solo per rows_ == columns_
  for (std::size_t r = 0; r < rows_; ++r)
    for (std::size_t c = 0; c < columns_; ++c)
    {
      Real tmp = getValue(r, c);
      setValue(r, c, getValue(c, r));
      setValue(c, r, tmp);
    }
  return *this;
}

std::vector<Real> Matrix::column(std::size_t c)
{
  std::vector<Real> col(rows_);

  for (std::size_t r = 0; r < rows_; ++r)
    col[r] = getValue(r, c);
  return col;
}

Real Matrix::max() const
{
  throw std::exception();
//  return *std::max_element(values_.begin(), values_.end());
}

Real Matrix::min() const
{
  throw std::exception();
//  return *std::min_element(values_.begin(), values_.end());
}

Matrix& Matrix::operator+=(const Matrix& matrix)
{
  if (!(rows_ == matrix.rows_ && columns_ == matrix.columns_))
    throw std::invalid_argument("Cannot sum matrixes with different size");
  
  for (std::size_t r = 0; r < rows_; ++r)
    for (std::size_t c = 0; c < columns_; ++c)
      setValue(r, c, (getValue(r, c) + matrix(r, c)));
  return *this;
}

Matrix& Matrix::operator-=(const Matrix& matrix)
{
  if (!(rows_ == matrix.rows_ && columns_ == matrix.columns_))
    throw std::invalid_argument("Cannot sum matrixes with different size");

  for (std::size_t r = 0; r < rows_; ++r)
    for (std::size_t c = 0; c < columns_; ++c)
      setValue(r, c, (getValue(r, c) - matrix(r, c)));
  return *this;
}

Matrix& Matrix::operator+=(Real value)
{
  for (std::size_t r = 0; r < rows_; ++r)
    for (std::size_t c = 0; c < columns_; ++c)
      setValue(r, c, getValue(r, c) + value);
  return *this;
}

Matrix& Matrix::operator-=(Real value)
{
  for (std::size_t r = 0; r < rows_; ++r)
    for (std::size_t c = 0; c < columns_; ++c)
      setValue(r, c, getValue(r, c) - value);
  return *this;
}

Matrix& Matrix::operator*=(Real value)
{
  for (std::size_t r = 0; r < rows_; ++r)
    for (std::size_t c = 0; c < columns_; ++c)
      setValue(r, c, getValue(r, c) * value);
  return *this;
}

Matrix& Matrix::operator/=(Real value)
{
  if (!(value > 0.0) && !(value < 0.0))
    throw std::invalid_argument("Error: division by 0");

  Real valueInverse = 1.0 / value;
  for (std::size_t r = 0; r < rows_; ++r)
    for (std::size_t c = 0; c < columns_; ++c)
      setValue(r, c, getValue(r, c) * valueInverse);
  return *this;
}

Matrix& Matrix::operator^=(Real value)
{
  for (std::size_t r = 0; r < rows_; ++r)
    for (std::size_t c = 0; c < columns_; ++c)
      setValue(r, c, pow(getValue(r, c), value));
  return *this;
}

// -------------
// Operators

Matrix operator+(const Matrix& left, const Matrix& right)
{
  if (!(left.rows_ == right.rows_ && left.columns_ == right.columns_))
    throw std::invalid_argument("Cannot sum matrixes with different size");

  Matrix result(left.rows_, left.columns_);
  result += right;
  return result;
}

Matrix operator-(const Matrix& left, const Matrix& right)
{
  if (!(left.rows_ == right.rows_ && left.columns_ == right.columns_))
    throw std::invalid_argument("Cannot subtract matrixes with different size");

  Matrix result(left.rows_, right.columns_);
  result -= right;
  return result;
}

Matrix operator*(const Matrix& left, const Matrix& right)
{
  if (left.columns_ != right.rows_)
    throw std::invalid_argument("Matrixes cannot be multiplied");

  Matrix result(left.rows_, right.columns_);

  for (std::size_t r = 0; r < left.rows_; ++r)
    for (std::size_t c = 0; c < right.columns_; ++c)
      for (std::size_t h = 0; h < left.columns_; ++h)
        result(r, c) += left(r, h) * right(h, c);
  return result;
}

Matrix operator+(const Matrix& matrix, Real value)
{
  Matrix result(matrix);
  result += value;
  return result;
}

Matrix operator-(const Matrix& matrix, Real value)
{
  Matrix result(matrix);
  result -= value;
  return result;
}

Matrix operator*(const Matrix& matrix, Real value)
{
  Matrix result(matrix);
  result *= value;
  return result;
}

Matrix operator/(const Matrix& matrix, Real value)
{
  if (!(value > 0.0) && !(value < 0.0))
    throw std::invalid_argument("Error: division by 0");

  Matrix result(matrix);
  result /= value;
  return result;
}


// Implementation classes

Matrix::DiagonalImpl::DiagonalImpl(std::size_t size)
  : values_(size)
{
}

Matrix::DiagonalImpl::DiagonalImpl(std::size_t size, const std::vector<Real>& diagonalValues)
  : values_(diagonalValues)
{
  if (size != diagonalValues.size())
    throw std::invalid_argument("");
}

Real& Matrix::DiagonalImpl::operator()(std::size_t row, std::size_t column)
{
  if (row != column)
    throw std::logic_error("Cannot set out of diagonal element in a diagonal matrix");
  return values_[row];
}

Real Matrix::DiagonalImpl::operator()(std::size_t row, std::size_t column) const
{
  return row == column ? values_[row] : 0;
}


Matrix::SimmetricImpl::SimmetricImpl(std::size_t size)
  : values_((size*(size+1))/2)
{
}

Matrix::SimmetricImpl::SimmetricImpl(std::size_t size, const std::vector<util::Real>& upperValues)
  : values_(upperValues)
{
  if (values_.size() != (size*(size+1))/2)
    throw std::invalid_argument("");
}

Real& Matrix::SimmetricImpl::operator()(std::size_t row, std::size_t column)
{
  std::size_t size = values_.size();
  std::size_t matrixSize = sqrt(size*2);
  std::size_t index = row < column
          ? (size - ((matrixSize - row)*(matrixSize - row + 1))/2) + column - row
          : (size - ((matrixSize - column)*(matrixSize - column + 1))/2) + row - column;
  return values_[index];
}

Real Matrix::SimmetricImpl::operator()(std::size_t row, std::size_t column) const
{
  std::size_t size = values_.size();
  std::size_t matrixSize = sqrt(size*2);
  std::size_t index = row < column
          ? (size - ((matrixSize - row)*(matrixSize - row + 1))/2) + column - row
          : (size - ((matrixSize - column)*(matrixSize - column + 1))/2) + row - column;
  return values_[index];
}


Matrix::GenericImpl::GenericImpl(std::size_t rows, std::size_t cols)
  : cols_(cols)
  , values_(rows*cols)
{}

Real& Matrix::GenericImpl::operator()(std::size_t row, std::size_t column)
{
  return values_[row*cols_ + column];
}

Real Matrix::GenericImpl::operator()(std::size_t row, std::size_t column) const
{
  return values_[row*cols_ + column];
}


Matrix::SparseImpl::SparseImpl(std::size_t rows, std::size_t cols)
  : rows_(rows), cols_(cols)
{
}

Real& Matrix::SparseImpl::operator()(std::size_t row, std::size_t column)
{
  return values_[std::make_pair(row, column)];
}

Real Matrix::SparseImpl::operator()(std::size_t row, std::size_t column) const
{
  std::map<std::pair<std::size_t, std::size_t>, Real>::const_iterator it
          = values_.find(std::make_pair(row, column));
  return it != values_.end() ? it->second : 0;
}

} // namespace util
